
#include "encoder_calibration.h"

CAL_HandleTypeDef calHandle;

int cycleDataAverage(const unsigned short * _data, unsigned short _length, int32_t _cyc)
{
    int sumData = 0;
    int subData;
    int diffData;

    sumData += (int)_data[0];
    for (unsigned short i = 1; i < _length; i++) {
        diffData = (int)_data[i];
        subData = (int)_data[i] - (int)_data[0];
        if (subData > (_cyc >> 1)) diffData = (int)_data[i] - _cyc;
        if (subData < (-_cyc >> 1)) diffData = (int)_data[i] + _cyc;
        sumData += diffData;
    }

    sumData = sumData / _length;

    if (sumData < 0) sumData += _cyc;
    if (sumData > _cyc) sumData -= _cyc;

    return sumData;
}

int cycleAverage(int _a, int _b, int _cyc)
{
    int sub_data;
    int ave_data;

    sub_data = _a - _b;
    ave_data = (_a + _b) >> 1;

    if (abs(sub_data) > (_cyc >> 1))
    {
        if (ave_data >= (_cyc >> 1)) ave_data -= (_cyc >> 1);
        else ave_data += (_cyc >> 1);
    }
    return ave_data;
}

int cycleSubtract(int _a, int _b, int _cyc)
{
    int sub_data;

    sub_data = _a - _b;
    if (sub_data > (_cyc >> 1)) sub_data -= _cyc;
    if (sub_data < (-_cyc >> 1)) sub_data += _cyc;
    return sub_data;
}

unsigned int cycleMod(uint32_t _a, uint32_t _b)
{
    return (_a + _b) % _b;
}

void encoderCALInit()
{
    calHandle.isTriggered        = false;
    calHandle.state              = CAL_CONTROL_DISABLE;
    calHandle.goPosition         = 0;
    calHandle.sampleRawCount     = 0;
    calHandle.sampleAverageCount = 0;
    calHandle.errorCode          = CAL_NO_ERROR;
    calHandle.errorData          = 0;
}

void encoderCALController()
{
    if (!calHandle.isTriggered)
        return;

    switch (calHandle.state) {
    case CAL_CONTROL_DISABLE:
        TB67H450_SetFocCurrentVector(calHandle.goPosition, CAL_CURRENT);
        calHandle.state = CAL_FORWARD_PREPARE;
        calHandle.goPosition = MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
        break;

    case CAL_FORWARD_PREPARE:
        TB67H450_SetFocCurrentVector(calHandle.goPosition, CAL_CURRENT);
        calHandle.goPosition = calHandle.goPosition + AUTO_CAL_SPEED;

        if (calHandle.goPosition == (MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS * 2)) {
            calHandle.state = CAL_FORWARD_MEASURE;
            calHandle.goPosition = MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS;
        }
        break;

    case CAL_FORWARD_MEASURE:
        // 由于第一次执行到此处就会采集数据，所以最终会采集 201 个数据，即数组下标 0-200。
        if ((calHandle.goPosition % SOFT_DIVIDE_NUM) == 0) {
            calHandle.sampleRawData[calHandle.sampleRawCount] = angleData.rawAngle;
            calHandle.sampleRawCount++;

            if (calHandle.sampleRawCount == SAMPLE_COUNTS_PER_STEP) {
                
                // calHandle.sampleAverageCount = (calHandle.goPosition - MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS) / SOFT_DIVIDE_NUM;

                calHandle.sampleAverageDataForward[calHandle.sampleAverageCount] = 
                    cycleDataAverage(calHandle.sampleRawData, SAMPLE_COUNTS_PER_STEP, ENCODER_RANGE);

                calHandle.sampleAverageCount++;
                calHandle.sampleRawCount = 0;
                calHandle.goPosition = calHandle.goPosition + FINE_TUNE_CAL_SPEED;
            }
        } else
            calHandle.goPosition = calHandle.goPosition + FINE_TUNE_CAL_SPEED;

        TB67H450_SetFocCurrentVector(calHandle.goPosition, CAL_CURRENT);

        if (calHandle.goPosition > (MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS * 2)) {
            calHandle.state = CAL_BACKWARD_RETURN;
        }
        break;

    case CAL_BACKWARD_RETURN:
        calHandle.goPosition = calHandle.goPosition + FINE_TUNE_CAL_SPEED;
        TB67H450_SetFocCurrentVector(calHandle.goPosition, CAL_CURRENT);
        if (calHandle.goPosition == (MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS * 2 + SOFT_DIVIDE_NUM * 20)) {
            calHandle.state = CAL_BACKWARD_GAP_DISMISS;
        }
        break;

    case CAL_BACKWARD_GAP_DISMISS:
        calHandle.goPosition = calHandle.goPosition - FINE_TUNE_CAL_SPEED;
        TB67H450_SetFocCurrentVector(calHandle.goPosition, CAL_CURRENT);
        if (calHandle.goPosition == (MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS * 2)) {
            calHandle.state = CAL_BACKWARD_MEASURE;
            calHandle.sampleAverageCount = MOTOR_ONE_CIRCLE_HARD_STEPS;
        }
        break;

    case CAL_BACKWARD_MEASURE:
        // 由于第一次执行到此处就会采集数据，所以最终会采集 201 个数据，即数组下标 200-0。
        if ((calHandle.goPosition % SOFT_DIVIDE_NUM) == 0) {
            calHandle.sampleRawData[calHandle.sampleRawCount] = angleData.rawAngle;
            calHandle.sampleRawCount++;

            if (calHandle.sampleRawCount == SAMPLE_COUNTS_PER_STEP) {
                
                // calHandle.sampleAverageCount = (calHandle.goPosition - MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS) / SOFT_DIVIDE_NUM;

                calHandle.sampleAverageDataBackward[calHandle.sampleAverageCount] = 
                    cycleDataAverage(calHandle.sampleRawData, SAMPLE_COUNTS_PER_STEP, ENCODER_RANGE);

                calHandle.sampleAverageCount--;
                calHandle.sampleRawCount = 0;
                calHandle.goPosition = calHandle.goPosition - FINE_TUNE_CAL_SPEED;
            }
        } else
            calHandle.goPosition = calHandle.goPosition - FINE_TUNE_CAL_SPEED;

        TB67H450_SetFocCurrentVector(calHandle.goPosition, CAL_CURRENT);

        if (calHandle.goPosition < MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS) {
            calHandle.state = CAL_CALCULATING;
        }
        break;

    case CAL_CALCULATING:
        TB67H450_SetFocCurrentVector(0, 0);
#if 0
        calHandle.isTriggered = false;
        for (unsigned int i = 0; i < MOTOR_ONE_CIRCLE_HARD_STEPS; i++) {
            printf(
                "F[%d]:%d, B[%d]:%d\n", 
                i, calHandle.sampleAverageDataForward[i],
                i, calHandle.sampleAverageDataBackward[i]
            );
        }
#endif
        break;
    }
}

void encoderCALDataCheck()
{
    for (unsigned int i = 0; i < (MOTOR_ONE_CIRCLE_HARD_STEPS + 1); i++) {
        calHandle.sampleAverageDataForward[i] = (unsigned short)cycleAverage(
            (int)calHandle.sampleAverageDataForward[i], 
            (int)calHandle.sampleAverageDataBackward[i], 
            ENCODER_RANGE
        );
    }

    // 使用第一个数据和最后一个数据比较，判断数据方向性。
    int diff1 = cycleSubtract(
        calHandle.sampleAverageDataForward[0], 
        calHandle.sampleAverageDataForward[MOTOR_ONE_CIRCLE_HARD_STEPS - 1], 
        ENCODER_RANGE
    );
    if (diff1 == 0) {
        calHandle.errorCode = CAL_ERROR_AVERAGE_DIR;
        return;
    } else {
        calHandle.goDirection = (diff1 > 0) ? true : false;
    }

    int diff2 = 0;
    for (unsigned int i = 1; i < MOTOR_ONE_CIRCLE_HARD_STEPS; i++) {
        // 循环连续计算前后相邻两个数据的相差值，用于判断数据的连续性，比如数据的分布是否均匀，十分理想的条件下两个数据的相差值是 16384 / 200 = 81.92
        diff2 = cycleSubtract(
            calHandle.sampleAverageDataForward[i], 
            calHandle.sampleAverageDataForward[i - 1], 
            ENCODER_RANGE
        );
        
        // 相邻两次采集的数据相差过大。
        if (abs(diff2) > (ENCODER_RANGE / MOTOR_ONE_CIRCLE_HARD_STEPS * 3 / 2)) {
            calHandle.errorCode = CAL_ERROR_AVERAGE_CONTINUTY;
            calHandle.errorData = i;
            return;
        }

        // 相邻两次采集的数据相差差过小。
        if (abs(diff2) < (ENCODER_RANGE / MOTOR_ONE_CIRCLE_HARD_STEPS * 1 / 2)) {
            calHandle.errorCode = CAL_ERROR_AVERAGE_CONTINUTY;
            calHandle.errorData = i;
            return;
        }

        // 数据方向错误。
        if (diff2 == 0) {
            calHandle.errorCode = CAL_ERROR_AVERAGE_DIR;
            calHandle.errorData = i;
            return;
        }

        if ((diff2 > 0) && (!calHandle.goDirection)) {
            calHandle.errorCode = CAL_ERROR_AVERAGE_DIR;
            calHandle.errorData = i;
            return;
        }

        if ((diff2 < 0) && (calHandle.goDirection)) {
            calHandle.errorCode = CAL_ERROR_AVERAGE_DIR;
            calHandle.errorData = i;
            return;
        }
    }

    unsigned int stepNum = 0;
    int diff3 = 0;

    /**
     * 通过循环的方式依次比较相邻两个数据的插值以寻找前面采集到的 200 个磁编码值数据中的数据阶跃值，与阶跃位置。
     * 需要寻找阶跃值的原因是，在校准数据采集阶段，采集开始时电机轴所处的位置不一定为 0，即磁编码值不为 0 我们
     * 也没有将电机驱动到 0 处再开始采集。所以采集到的磁编码器数据不是按照从小到大或从大到小依次排列的，例如排列
     * 方式为 [8192, 8193, ..., 16383, 16384, 0, 1, ..., 8190, 8191] 这种排列方式。
     */
    if (calHandle.goDirection) {
        for (unsigned int i = 0; i < MOTOR_ONE_CIRCLE_HARD_STEPS; i++) {
            diff3 = (int)calHandle.sampleAverageDataForward[cycleMod(i + 1, MOTOR_ONE_CIRCLE_HARD_STEPS)] - 
                    (int)calHandle.sampleAverageDataForward[cycleMod(i + 0, MOTOR_ONE_CIRCLE_HARD_STEPS)];

            if (diff3 < 0) {
                stepNum++;
                calHandle.rcdX = i; // 使用区间前标
                calHandle.rcdY = (ENCODER_RANGE - 1) - 
                    calHandle.sampleAverageDataForward[cycleMod((calHandle.rcdX + 0), MOTOR_ONE_CIRCLE_HARD_STEPS)];
            }
        }
        if (stepNum != 1) {
            calHandle.errorCode = CAL_ERROR_PHASE_STEP;
            return;
        }
    } else {
        for (unsigned int i = 0; i < MOTOR_ONE_CIRCLE_HARD_STEPS; i++) {
            diff3 = (int)calHandle.sampleAverageDataForward[cycleMod(i + 1, MOTOR_ONE_CIRCLE_HARD_STEPS)] - 
                    (int)calHandle.sampleAverageDataForward[cycleMod(i + 0, MOTOR_ONE_CIRCLE_HARD_STEPS)];

            if (diff3 > 0) {
                stepNum++;
                calHandle.rcdX = i; // 使用区间前标
                calHandle.rcdY = (ENCODER_RANGE - 1) - 
                    calHandle.sampleAverageDataForward[cycleMod((calHandle.rcdX + 1), MOTOR_ONE_CIRCLE_HARD_STEPS)];
            }
        }
        if (stepNum != 1) {
            calHandle.errorCode = CAL_ERROR_PHASE_STEP;
            return;
        }
    }

    calHandle.errorCode = CAL_NO_ERROR;
}

void encoderCALDataCalc()
{
    /**
     * 校准的原理是利用前面采集到的 200 个数据(每 1.8° 采集一个数据)，进行线性插值处理，将 200 个
     * 数据插值到 16384 个数据（而插值后的数据范围为 0-512000，即 256 * 200 = 512000，就是步进
     * 电机单圈脉冲数），这样磁编码器量程范围内的每个编码值都有一个插值数据与之对应，校准后利用磁编
     * 码器编码值作为作为插值数据数组下标查表得出校准值即可。最终的思想就是获取磁编码器的读数再查表
     * 就可以知道步进电机的位置是多少个脉冲。
     */

    int dataI32;
    unsigned short dataU16;
    int stepX, stepY;

    if (calHandle.state != CAL_CALCULATING)
        return;

    encoderCALDataCheck();

    if (calHandle.errorCode != CAL_NO_ERROR)
        return;

    Stockpile_Flash_Data_Empty(&stockpile_quick_cali); // 擦除数据区
    Stockpile_Flash_Data_Begin(&stockpile_quick_cali); // 开始写数据区
    calHandle.resultNum = 0;

    if (calHandle.goDirection) {
        for (stepX = calHandle.rcdX; stepX < calHandle.rcdX + MOTOR_ONE_CIRCLE_HARD_STEPS + 1; stepX++) {
            dataI32 = cycleSubtract(
                calHandle.sampleAverageDataForward[cycleMod(stepX + 1, MOTOR_ONE_CIRCLE_HARD_STEPS)],
                calHandle.sampleAverageDataForward[cycleMod(stepX, MOTOR_ONE_CIRCLE_HARD_STEPS)],
                ENCODER_RANGE
            );

            if (stepX == calHandle.rcdX) { // 开始边缘
                for (stepY = calHandle.rcdY; stepY < dataI32; stepY++) {
                    dataU16 = cycleMod(
                        SOFT_DIVIDE_NUM * stepX + SOFT_DIVIDE_NUM * stepY / dataI32,
                        MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS
                    );

                    Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &dataU16, 1);
                    calHandle.resultNum++;
                }
            } else if (stepX == calHandle.rcdX + MOTOR_ONE_CIRCLE_HARD_STEPS) { // 结束边缘
                for (stepY = 0; stepY < calHandle.rcdY; stepY++) {
                    dataU16 = cycleMod(
                        SOFT_DIVIDE_NUM * stepX + SOFT_DIVIDE_NUM * stepY / dataI32,
                        MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS
                    );

                    Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &dataU16, 1);
                    calHandle.resultNum++;
                }
            } else { // 中间
                for (stepY = 0; stepY < dataI32; stepY++) {
                    dataU16 = cycleMod(
                        SOFT_DIVIDE_NUM * stepX + SOFT_DIVIDE_NUM * stepY / dataI32,
                        MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS
                    );

                    Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &dataU16, 1);
                    calHandle.resultNum++;
                }
            }
        }
    } else {
        for (stepX = calHandle.rcdX + MOTOR_ONE_CIRCLE_HARD_STEPS; stepX > calHandle.rcdX - 1; stepX--) {
            dataI32 = cycleSubtract(
                calHandle.sampleAverageDataForward[cycleMod(stepX, MOTOR_ONE_CIRCLE_HARD_STEPS)],
                calHandle.sampleAverageDataForward[cycleMod(stepX + 1, MOTOR_ONE_CIRCLE_HARD_STEPS)],
                ENCODER_RANGE
            );

            if (stepX == calHandle.rcdX + MOTOR_ONE_CIRCLE_HARD_STEPS) { // 开始边缘
                for (stepY = calHandle.rcdY; stepY < dataI32; stepY++) {
                    dataU16 = cycleMod(
                        SOFT_DIVIDE_NUM * (stepX + 1) - SOFT_DIVIDE_NUM * stepY / dataI32,
                        MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS
                    );

                    Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &dataU16, 1);
                    calHandle.resultNum++;
                }
            } else if (stepX == calHandle.rcdX) { // 结束边缘
                for (stepY = 0; stepY < calHandle.rcdY; stepY++) {
                    dataU16 = cycleMod(
                        SOFT_DIVIDE_NUM * (stepX + 1) - SOFT_DIVIDE_NUM * stepY / dataI32,
                        MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS
                    );

                    Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &dataU16, 1);
                    calHandle.resultNum++;
                }
            } else { // 中间
                for (stepY = 0; stepY < dataI32; stepY++) {
                    dataU16 = cycleMod(
                        SOFT_DIVIDE_NUM * (stepX + 1) - SOFT_DIVIDE_NUM * stepY / dataI32,
                        MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS
                    );

                    Stockpile_Flash_Data_Write_Data16(&stockpile_quick_cali, &dataU16, 1);
                    calHandle.resultNum++;
                }
            }
        }
    }

    Stockpile_Flash_Data_End(&stockpile_quick_cali); // 结束写数据区
    
    if (calHandle.resultNum != ENCODER_RANGE) // 校准后的数据个数错误
        calHandle.errorCode = CAL_ERROR_ANALYSIS_QUANTITY;

    if (calHandle.errorCode == CAL_NO_ERROR)
        angleData.rectifyValid = true;
    else {
        angleData.rectifyValid = false;
        Stockpile_Flash_Data_Empty(&stockpile_quick_cali);
    }

    calHandle.state = CAL_CONTROL_DISABLE;
    calHandle.isTriggered = false;
    HAL_NVIC_SystemReset(); // 软件复位主控芯片
}
